//
// Created by apple on 2025/4/20.
//
#include "pid.h"

float PID_Compute(PID_t *pid, float target, float measured)
{
    float error = target - measured;
    pid->integral += error;
    float derivative = error - pid->prev_error;
    pid->prev_error = error;

    float output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;
    return output;
}